import time
import serial
import threading
from weiyi import LE100485Sensor
from motor import LingkongMotor

class MeasurementSystem:
    def __init__(self, 
                 port='COM3',  # 共用一个串口
                 baudrate=115200,  # 统一波特率（必须与传感器、电机配置一致）
                 motor_id=0x01,
                 sensor_address=2):
        """初始化测量系统（共享串口）"""
        # 创建共享串口实例（关键：只打开一次）
        self.shared_ser = serial.Serial(
            port=port,
            baudrate=baudrate,
            parity=serial.PARITY_NONE,
            stopbits=serial.STOPBITS_ONE,
            bytesize=serial.EIGHTBITS,
            timeout=0.5
        )
        
        # 初始化电机（传入共享串口）
        self.joint_motor = LingkongMotor(
            motor_id=motor_id,
            baudrate=baudrate,
            ser=self.shared_ser  # 共享串口
        )
        
        # 初始化传感器（传入共享串口，确保波特率与共享串口一致）
        self.laser_sensor = LE100485Sensor(
            sensor_address=sensor_address,
            baudrate=baudrate,  # 必须与共享串口波特率相同
            ser=self.shared_ser  # 共享串口
        )
        
        self.measurement_data = []
        self.is_measuring = False
        self.serial_lock = threading.Lock()  # 串口互斥锁（防止同时读写）

    def collect_data_continuously(self, interval=0.01):
        """带互斥锁的连续采集"""
        while self.is_measuring:
            with self.serial_lock:  # 电机操作加锁
                _, current_angle = self.joint_motor.read_current_multi_turn_angle()
            
            time.sleep(0.001)  # 微小延迟避免冲突
            
            with self.serial_lock:  # 传感器操作加锁
                distance = self.laser_sensor.read_distance()
            
            if current_angle is not None and distance is not None:
                normalized_angle = current_angle % 360
                self.measurement_data.append((normalized_angle, distance))
                print(f"角度: {normalized_angle:.2f}°  距离: {distance:.5f}mm", end='\r')
            
            time.sleep(interval)

    def start_measurement(self):
        """测量流程（保持原有逻辑，添加互斥锁）"""
        try:
            print("正在将关节电机回归零位...")
            with self.serial_lock:
                zero_reached = self.joint_motor.move_to_multi_turn_zero()
            if not zero_reached:
                print("电机回零失败，无法进行测量")
                return False

            self.measurement_data = []
            self.is_measuring = True
            data_thread = threading.Thread(
                target=self.collect_data_continuously,
                args=(0.01,),
                daemon=True
            )
            data_thread.start()

            print("\n开始旋转并采集数据...")
            with self.serial_lock:
                rotate_success = self.joint_motor.rotate_360_from_zero()
            if not rotate_success:
                print("电机旋转失败")
                self.is_measuring = False
                return False

            self.is_measuring = False
            data_thread.join()
            print("\n数据采集完成")

            print("回归零位...")
            with self.serial_lock:
                self.joint_motor.move_to_multi_turn_zero()

            self.save_measurement_data()
            return True

        except Exception as e:
            print(f"测量过程出错: {str(e)}")
            self.is_measuring = False
            return False

    def save_measurement_data(self, filename="measurement_data.txt"):
        with open(filename, 'w') as f:
            f.write("角度(度),距离(mm)\n")
            for angle, distance in self.measurement_data:
                f.write(f"{angle:.4f},{distance:.5f}\n")
        print(f"数据已保存到 {filename}，共 {len(self.measurement_data)} 条记录")

    def close(self):
        """统一关闭共享串口"""
        if self.shared_ser.is_open:
            self.shared_ser.close()
        print("系统资源已释放")

if __name__ == "__main__":
    # 初始化系统（共用COM3，波特率必须统一）
    measurement_system = MeasurementSystem(
        port='COM3',
        baudrate=115200,  # 确保传感器已配置为115200波特率
        motor_id=0x01,
        sensor_address=2
    )

    try:
        measurement_system.start_measurement()
    except KeyboardInterrupt:
        print("\n程序被手动中断")
    finally:
        measurement_system.close()